#include "ros/ros.h"
#include "nav_msgs/Odometry.h"
#include <cmath>

double start_x, start_y; // 起始位置坐标
bool isFirstMessage = true; // 标志位，用于检查是否是第一条消息

void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
{
	ROS_INFO("test---1");
    double current_x = msg->pose.pose.position.x;
    double current_y = msg->pose.pose.position.y;

    if (isFirstMessage) // 在第一条消息到达时设置起始位置
    {
        start_x = current_x;
        start_y = current_y;
        isFirstMessage = false; // 设置标志，表示起始位置已设定
        ROS_INFO("init_x: %.3f,init_y: %.3f", start_x, start_y);
    }
    
    double move_distance = sqrt(pow(current_x - start_x, 2) + pow(current_y - start_y, 2));
    ROS_INFO("move_distance = %.3f m", move_distance);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "odom_d");
    ros::NodeHandle n;

    ROS_INFO("init_success");

    ros::Subscriber sub = n.subscribe("/odom", 1, odomCallback);

    ros::spin();
    return 0;
}